#include "userMain.h"
#include "userBMA253driver.h"
#include "math.h"

#define     BMA253_WRITE_ADDR       0x30
#define     BMA253_READ_ADDR        0x31

#define     REG_BGW_CHIPID          0x00
#define     REG_ACCD_X_LSB          0x02
#define     REG_ACCD_X_MSB          0x03
#define     REG_ACCD_Y_LSB          0x04
#define     REG_ACCD_Y_MSB          0x05
#define     REG_ACCD_Z_LSB          0x06
#define     REG_ACCD_Z_MSB          0x07
#define     REG_ACCD_TEMP           0x08
#define     REG_INT_STATUS_0        0x09
#define     REG_PMU_RANGE           0x0F
#define     REG_PMU_BW              0x10

#define     SUCCESS                 0x01
#define     FAIL                    0x00
uint8_t userBMA253readRegister(uint8_t regAddr,uint8_t *regValue)
{
    DE_Cx_IIC_INITIAL;
    DE_Cx_IIC_START;
    Cx_Write8Bit(BMA253_WRITE_ADDR);
    if(DE_Cx_TESTACK)
    {
        DE_Cx_IIC_STOP;
        DE_Cx_IIC_END;
        return FAIL;
    }
    Cx_Write8Bit(regAddr);
    if(DE_Cx_TESTACK)
    {
        DE_Cx_IIC_STOP;
        DE_Cx_IIC_END;
        return FAIL;
    }
    DE_Cx_IIC_START;
    Cx_Write8Bit(BMA253_READ_ADDR);
    if(DE_Cx_TESTACK)
    {
        DE_Cx_IIC_STOP;
        DE_Cx_IIC_END;
        return FAIL;
    }
    *regValue= DE_Cx_READ8BIT;
    DE_Cx_NOACK;
    DE_Cx_IIC_STOP;
    DE_Cx_IIC_END;
    return SUCCESS;
}

uint8_t userBMA253writeRegister(uint8_t regAddr,uint8_t regValue)
{
    DE_Cx_IIC_INITIAL;
    DE_Cx_IIC_START;
    Cx_Write8Bit(BMA253_WRITE_ADDR);
    if(DE_Cx_TESTACK)
    {
        DE_Cx_IIC_STOP;
        DE_Cx_IIC_END;
        return FAIL;
    }
    Cx_Write8Bit(regAddr);
    if(DE_Cx_TESTACK)
    {
        DE_Cx_IIC_STOP;
        DE_Cx_IIC_END;
        return FAIL;
    }
    Cx_Write8Bit(regValue);
    if(DE_Cx_TESTACK)
    {
        DE_Cx_IIC_STOP;
        DE_Cx_IIC_END;
        return FAIL;
    }
    DE_Cx_IIC_STOP;
    DE_Cx_IIC_END;
    return SUCCESS;
}
uint8_t userRegValue = 0;
uint8_t userBMA253initialFunc(void)
{
    if(userBMA253readRegister(REG_BGW_CHIPID,&userRegValue) == FAIL)
    {
        return FAIL;
    }
    if(userRegValue != 0xFA)        //read chipID if right
    {
        return FAIL;
    }
    if(userBMA253writeRegister(REG_PMU_RANGE,0x03)!= SUCCESS)   //set 2g
    {
        return FAIL;
    }
    if(userBMA253writeRegister(REG_PMU_BW,0x0A)!= SUCCESS)      //set 31.25Hz
    {
        return FAIL;
    }
    if(userBMA253writeRegister(REG_INT_STATUS_0,0x00)!= SUCCESS)      //no interupt
    {
        return FAIL;
    }
    return SUCCESS;
}

uint16_t BMA_X_Value,BMA_Y_Value,BMA_Z_Value;
int16_t uXvalue,uYvalue,uZvalue;
int16_t uXvaluePre,uYvaluePre,uZvaluePre;
uint16_t uXoffset,uYoffset,uZoffset;
uint32_t uSumOffset;
uint16_t uModOffset;
uint8_t BMA_PowerFlag = 1;
uint8_t userGetBMA_ACC_XYZ_Func(void)
{
    uint8_t userBuff[6];
    uint8_t user_i;
    DE_Cx_IIC_INITIAL;
    DE_Cx_IIC_START;
    Cx_Write8Bit(BMA253_WRITE_ADDR);
    if(DE_Cx_TESTACK)
    {
        DE_Cx_IIC_STOP;
        DE_Cx_IIC_END;
        return FAIL;
    }
    Cx_Write8Bit(0x02);
    if(DE_Cx_TESTACK)
    {
        DE_Cx_IIC_STOP;
        DE_Cx_IIC_END;
        return FAIL;
    }
    DE_Cx_IIC_START;
    Cx_Write8Bit(BMA253_READ_ADDR);
    if(DE_Cx_TESTACK)
    {
        DE_Cx_IIC_STOP;
        DE_Cx_IIC_END;
        return FAIL;
    }
    for(user_i=0;user_i<5;user_i++)
    {
        userBuff[user_i] = DE_Cx_READ8BIT;
        DE_Cx_ACK;
    }
    userBuff[user_i] = DE_Cx_READ8BIT;
    DE_Cx_NOACK;
    DE_Cx_IIC_STOP;
    DE_Cx_IIC_END;
    BMA_X_Value = userBuff[1]<<8;
    BMA_X_Value |= userBuff[0] & 0xF0;
    BMA_X_Value = BMA_X_Value >> 4;
    
    BMA_Y_Value = userBuff[3]<<8;
    BMA_Y_Value |= userBuff[2] & 0xF0;
    BMA_Y_Value = BMA_Y_Value >> 4;
    
    BMA_Z_Value = userBuff[5]<<8;
    BMA_Z_Value |= userBuff[4] & 0xF0;
    BMA_Z_Value = BMA_Z_Value >> 4;
    
    if(BMA_X_Value > 0x7FF)
    {
      uXvalue = 0x1000 - BMA_X_Value;
    }
    else
    {
      uXvalue = BMA_X_Value;
    }
    uXvalue = uXvalue * 10 ;
    uXvalue >>= 7;
    if(BMA_Y_Value > 0x7FF)
    {
      uYvalue = 0x1000 - BMA_Y_Value;
    }
    else
    {
      uYvalue = BMA_Y_Value;
    }
    uYvalue = uYvalue * 10;
    uYvalue >>= 7;
    if(BMA_Z_Value > 0x7FF)
    {
      uZvalue = 0x1000 - BMA_Z_Value;
    }
    else
    {
      uZvalue = BMA_Z_Value;
    }
    uZvalue = uZvalue * 10;
    uZvalue >>= 7;
    if(BMA_PowerFlag)
    {
      BMA_PowerFlag = 0;
      uXvaluePre = uXvalue;
      uYvaluePre = uYvalue;
      uZvaluePre = uZvalue;
      uModOffset = 0;
      return SUCCESS;
    }
    uXoffset = (uint16_t)fabs(uXvaluePre - uXvalue);
    uYoffset = (uint16_t)fabs(uYvaluePre - uYvalue);
    uZoffset = (uint16_t)fabs(uZvaluePre - uZvalue);
    
    uSumOffset = uXoffset * uXoffset;
    uSumOffset += uYoffset * uYoffset;
    uSumOffset += uZoffset * uZoffset;
    uModOffset = (uint16_t)sqrt(uSumOffset);
    if(uModOffset>2)
      _NOP;
    return SUCCESS;
}

uint8_t uBmaDetectFlag = 0;
uint8_t uBmaDetectCycleCnt = 0;
void userGetDataFromBMA253Func(void)
{
    if(uBmaDetectCycleCnt < 10)
    {
      uBmaDetectCycleCnt++;
      return ;
    }
    if(uBmaDetectFlag) return ;
    uBmaDetectCycleCnt = 0;
    //userBMA253initialFunc();
    userGetBMA_ACC_XYZ_Func();
    userSendAccInfoFunc();
}

